/*******************************************************************************
 *                                                                              *
 *   PrimeSense NITE 1.3 - Point Viewer Sample                                  *
 *   Copyright (C) 2010 PrimeSense Ltd.                                         *
 *                                                                              *
 *******************************************************************************/

// Headers for OpenNI
#include <XnOpenNI.h>
#include <XnCppWrapper.h>
#include <XnHash.h>
#include <XnLog.h>

// Header for NITE
#include "XnVNite.h"
// local header
#include "PointDrawer.h"

#include "Drawer.h"


#define CHECK_RC(rc, what)											\
	if (rc != XN_STATUS_OK)											\
	{																\
		printf("%s failed: %s\n", what, xnGetStatusString(rc));		\
		return rc;													\
	}

#define CHECK_ERRORS(rc, errors, what)		\
	if (rc == XN_STATUS_NO_NODE_PRESENT)	\
	{										\
		XnChar strError[1024];				\
		errors.ToString(strError, 1024);	\
		printf("%s\n", strError);			\
		return (rc);						\
	}

#ifdef USE_GLUT
#if (XN_PLATFORM == XN_PLATFORM_MACOSX)
#include <GLUT/glut.h>
#else
#include <GL/glut.h>
#endif
#elif defined(USE_GLES)
#include "opengles.h"
#include "kbhit.h"
#endif
#include "signal_catch.h"

#ifdef USE_GLES
static EGLDisplay display = EGL_NO_DISPLAY;
static EGLSurface surface = EGL_NO_SURFACE;
static EGLContext context = EGL_NO_CONTEXT;
#endif

// OpenNI objects
xn::Context g_Context;
xn::ScriptNode g_ScriptNode;
xn::DepthGenerator g_DepthGenerator;
xn::HandsGenerator g_HandsGenerator;
xn::GestureGenerator g_GestureGenerator;

// NITE objects
XnVSessionManager* g_pSessionManager;
XnVFlowRouter* g_pFlowRouter;

// the drawer
XnVPointDrawer* g_pDrawer;

#define GL_WIN_SIZE_X 720
#define GL_WIN_SIZE_Y 480

// Draw the depth map?
XnBool g_bDrawDepthMap = true;
XnBool g_bPrintFrameID = false;
// Use smoothing?
XnFloat g_fSmoothing = 0.0f;
XnBool g_bPause = false;
XnBool g_bQuit = false;

SessionState g_SessionState = NOT_IN_SESSION;

//Vars for tests:
extern float x;
extern float y;

void CleanupExit() {
    g_ScriptNode.Release();
    g_DepthGenerator.Release();
    g_HandsGenerator.Release();
    g_GestureGenerator.Release();
    g_Context.Release();

    exit(1);
}

// Callback for when the focus is in progress

void XN_CALLBACK_TYPE FocusProgress(const XnChar* strFocus, const XnPoint3D& ptPosition, XnFloat fProgress, void* UserCxt) {
    //	printf("Focus progress: %s @(%f,%f,%f): %f\n", strFocus, ptPosition.X, ptPosition.Y, ptPosition.Z, fProgress);
}
// callback for session start

void XN_CALLBACK_TYPE SessionStarting(const XnPoint3D& ptPosition, void* UserCxt) {
    printf("Session start: (%f,%f,%f)\n", ptPosition.X, ptPosition.Y, ptPosition.Z);
    g_SessionState = IN_SESSION;
}
// Callback for session end

void XN_CALLBACK_TYPE SessionEnding(void* UserCxt) {
    printf("Session end\n");
    g_SessionState = NOT_IN_SESSION;
}

void XN_CALLBACK_TYPE NoHands(void* UserCxt) {
    if (g_SessionState != NOT_IN_SESSION) {
        printf("Quick refocus\n");
        g_SessionState = QUICK_REFOCUS;
    }
}

void XN_CALLBACK_TYPE TouchingCallback(xn::HandTouchingFOVEdgeCapability& generator, XnUserID id, const XnPoint3D* pPosition, XnFloat fTime, XnDirection eDir, void* pCookie) {
    g_pDrawer->SetTouchingFOVEdge(id);
}
/*
void XN_CALLBACK_TYPE MyGestureInProgress(xn::GestureGenerator& generator, const XnChar* strGesture, const XnPoint3D* pPosition, void* pCookie)
{
        printf("Gesture %s in progress\n", strGesture);
}
void XN_CALLBACK_TYPE MyGestureReady(xn::GestureGenerator& generator, const XnChar* strGesture, const XnPoint3D* pPosition, void* pCookie)
{
        printf("Gesture %s ready for next stage\n", strGesture);
}
 */

float _angle = 30.0f;

//Draws the 3D scene

void glutDisplay() {

    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

    glClearColor(0.8f, 0.8f, 0.8f, 1.0f);

    glMatrixMode(GL_MODELVIEW); //Switch to the drawing perspective
    glLoadIdentity(); //Reset the drawing perspective
    glTranslatef(0.0f, 0.0f, -15.0f);

    glColor3f(1.0f, 1.0f, 1.0f);
    glBegin(GL_QUADS);

    glNormal3f(0.0, 1.0f, 0.0f);
    glTexCoord2f(0.0f, 1.0f);
    glVertex3f(-3.0f, 5.0f, 0.0f);

    glTexCoord2f(1.0f, 1.0f);
    glVertex3f(3.0f, 5.0f, 0.0f);

    glTexCoord2f(1.0f, 0.0f);
    glVertex3f(3.0f, -5.0f, 0.0f);

    glTexCoord2f(0.0f, 0.0f);
    glVertex3f(-3.0f, -5.0f, 0.0f);
    glEnd();
    
    Drawer::get().drawHud();

//    if(x!=0.0)
//        printf("%f\n", Drawer::get().pointer.X);
    
    // Read next available data
    g_Context.WaitOneUpdateAll(g_DepthGenerator);
    // Update NITE tree
    g_pSessionManager->Update(&g_Context);


    glutSwapBuffers();
}

// this function is called each frame

void glutDisplay2(void) {

    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

    // Setup the OpenGL viewpoint
    glMatrixMode(GL_PROJECTION);
    glPushMatrix();
    glLoadIdentity();

    XnMapOutputMode mode;
    g_DepthGenerator.GetMapOutputMode(mode);
#ifdef USE_GLUT
    glOrtho(0, mode.nXRes, mode.nYRes, 0, -1.0, 1.0);
#elif defined(USE_GLES)
    glOrthof(0, mode.nXRes, mode.nYRes, 0, -1.0, 1.0);
#endif

    glDisable(GL_TEXTURE_2D);

    //    glColor3f(1.0f, 0.0f, 0.0f);
    //    glutSolidCube(3);
    //
    glTranslatef(0.0f, 1.0f, -0.9f);


    if (!g_bPause) {
        // Read next available data
        g_Context.WaitOneUpdateAll(g_DepthGenerator);
        // Update NITE tree
        g_pSessionManager->Update(&g_Context);

#ifdef USE_GLUT
        //        PrintSessionState(g_SessionState);
#endif
    }

#ifdef USE_GLUT
    glutSwapBuffers();
#endif
}

#ifdef USE_GLUT

void glutIdle(void) {
    if (g_bQuit) {
        CleanupExit();
    }

    // Display the frame
    glutPostRedisplay();
}

void glutKeyboard(unsigned char key, int x, int y) {
    switch (key) {
        case 27:
            // Exit
            CleanupExit();
        case'p':
            // Toggle pause
            g_bPause = !g_bPause;
            break;
        case 'd':
            // Toggle drawing of the depth map
            g_bDrawDepthMap = !g_bDrawDepthMap;
            g_pDrawer->SetDepthMap(g_bDrawDepthMap);
            break;
        case 'f':
            g_bPrintFrameID = !g_bPrintFrameID;
            g_pDrawer->SetFrameID(g_bPrintFrameID);
            break;
        case 's':
            // Toggle smoothing
            if (g_fSmoothing == 0)
                g_fSmoothing = 0.1;
            else
                g_fSmoothing = 0;
            g_HandsGenerator.SetSmoothing(g_fSmoothing);
            break;
        case 'e':
            // end current session
            g_pSessionManager->EndSession();
            break;
    }
}

void handleResize(int w, int h) {
    glViewport(0, 0, w, h);
    glMatrixMode(GL_PROJECTION);
    glLoadIdentity();
    gluPerspective(45.0, (double) w / (double) h, 1.0, 200.0);
    Drawer::get().windowW = w;
    Drawer::get().windowH = h;
}

void glInit(int * pargc, char ** argv) {
    glutInit(pargc, argv);
    glutInitDisplayMode(GLUT_RGB | GLUT_DOUBLE | GLUT_DEPTH);
    glutInitWindowSize(GL_WIN_SIZE_X, GL_WIN_SIZE_Y);
    glutCreateWindow("eOffer Kinected");
    //glutFullScreen();
    glutSetCursor(GLUT_CURSOR_NONE);

    glutKeyboardFunc(glutKeyboard);
    glutDisplayFunc(glutDisplay);
    glutIdleFunc(glutIdle);
    glutReshapeFunc(handleResize);

    glDisable(GL_DEPTH_TEST);
    //    glEnable(GL_DEPTH_TEST);
    glEnable(GL_TEXTURE_2D);

    glEnableClientState(GL_VERTEX_ARRAY);
    glDisableClientState(GL_COLOR_ARRAY);
}
#endif

void XN_CALLBACK_TYPE GestureIntermediateStageCompletedHandler(xn::GestureGenerator& generator, const XnChar* strGesture, const XnPoint3D* pPosition, void* pCookie) {
    printf("Gesture %s: Intermediate stage complete (%f,%f,%f)\n", strGesture, pPosition->X, pPosition->Y, pPosition->Z);
}

void XN_CALLBACK_TYPE GestureReadyForNextIntermediateStageHandler(xn::GestureGenerator& generator, const XnChar* strGesture, const XnPoint3D* pPosition, void* pCookie) {
    printf("Gesture %s: Ready for next intermediate stage (%f,%f,%f)\n", strGesture, pPosition->X, pPosition->Y, pPosition->Z);
}

void XN_CALLBACK_TYPE GestureProgressHandler(xn::GestureGenerator& generator, const XnChar* strGesture, const XnPoint3D* pPosition, XnFloat fProgress, void* pCookie) {
    printf("Gesture %s progress: %f (%f,%f,%f)\n", strGesture, fProgress, pPosition->X, pPosition->Y, pPosition->Z);
}


// xml to initialize OpenNI
#define SAMPLE_XML_PATH "./Data/Sample-Tracking.xml"

int main(int argc, char ** argv) {
    XnStatus rc = XN_STATUS_OK;
    xn::EnumerationErrors errors;

    // Initialize OpenNI
    rc = g_Context.InitFromXmlFile(SAMPLE_XML_PATH, g_ScriptNode, &errors);
    CHECK_ERRORS(rc, errors, "InitFromXmlFile");
    CHECK_RC(rc, "InitFromXmlFile");

    rc = g_Context.FindExistingNode(XN_NODE_TYPE_DEPTH, g_DepthGenerator);
    CHECK_RC(rc, "Find depth generator");
    rc = g_Context.FindExistingNode(XN_NODE_TYPE_HANDS, g_HandsGenerator);
    CHECK_RC(rc, "Find hands generator");
    rc = g_Context.FindExistingNode(XN_NODE_TYPE_GESTURE, g_GestureGenerator);
    CHECK_RC(rc, "Find gesture generator");

    XnCallbackHandle h;
    if (g_HandsGenerator.IsCapabilitySupported(XN_CAPABILITY_HAND_TOUCHING_FOV_EDGE)) {
        g_HandsGenerator.GetHandTouchingFOVEdgeCap().RegisterToHandTouchingFOVEdge(TouchingCallback, NULL, h);
    }

    XnCallbackHandle hGestureIntermediateStageCompleted, hGestureProgress, hGestureReadyForNextIntermediateStage;
    g_GestureGenerator.RegisterToGestureIntermediateStageCompleted(GestureIntermediateStageCompletedHandler, NULL, hGestureIntermediateStageCompleted);
    g_GestureGenerator.RegisterToGestureReadyForNextIntermediateStage(GestureReadyForNextIntermediateStageHandler, NULL, hGestureReadyForNextIntermediateStage);
    g_GestureGenerator.RegisterGestureCallbacks(NULL, GestureProgressHandler, NULL, hGestureProgress);


    // Create NITE objects
    g_pSessionManager = new XnVSessionManager;
    rc = g_pSessionManager->Initialize(&g_Context, "Click,Wave", "RaiseHand");
    CHECK_RC(rc, "SessionManager::Initialize");

    g_pSessionManager->RegisterSession(NULL, SessionStarting, SessionEnding, FocusProgress);

    g_pDrawer = new XnVPointDrawer(20, g_DepthGenerator);
    g_pFlowRouter = new XnVFlowRouter;
    g_pFlowRouter->SetActive(g_pDrawer);

    g_pSessionManager->AddListener(g_pFlowRouter);

    g_pDrawer->RegisterNoPoints(NULL, NoHands);
    g_pDrawer->SetDepthMap(g_bDrawDepthMap);

    // Initialization done. Start generating
    rc = g_Context.StartGeneratingAll();
    CHECK_RC(rc, "StartGenerating");

    // Mainloop
#ifdef USE_GLUT

    glInit(&argc, argv);
    glutMainLoop();

#elif defined(USE_GLES)
    if (!opengles_init(GL_WIN_SIZE_X, GL_WIN_SIZE_Y, &display, &surface, &context)) {
        printf("Error initializing opengles\n");
        CleanupExit();
    }
    glDisable(GL_DEPTH_TEST);
    glEnable(GL_TEXTURE_2D);
    glEnableClientState(GL_VERTEX_ARRAY);
    glDisableClientState(GL_COLOR_ARRAY);

    while ((!_kbhit()) && (!g_bQuit)) {
        glutDisplay();
        eglSwapBuffers(display, surface);
    }
    opengles_shutdown(display, surface, context);

    CleanupExit();
#endif
}